import argparse
import sys
import time
from multiprocessing import Process, Queue
from commands.invoker import CommandInvoker
from commands.start import StartCommand
from commands.stop import StopCommand
from commands.calibrate import CalibrateCommand, StereoCalibrateCommand
from commands.show_image import (
    ShowImagesCommand,
    ShowUndistImagesCommand,
    ShowDisparityCommand,
)


def create_parser():
    """创建命令行解析器"""
    parser = argparse.ArgumentParser(description="双目视觉系统")
    subparsers = parser.add_subparsers(dest="command", help="可用命令")

    # --start 子命令
    subparsers.add_parser("start", help="启动相机并捕获图像")

    # --stop 子命令
    subparsers.add_parser("stop", help="停止相机")

    # --show 子命令
    subparsers.add_parser("show", help="显示相机图像")

    # --calibrate 子命令
    parser_calibrate = subparsers.add_parser("cal", help="校准相机")
    parser_calibrate.add_argument(
        "--cols", type=int, required=True, help="棋盘格的列数"
    )
    parser_calibrate.add_argument(
        "--rows", type=int, required=True, help="棋盘格的行数"
    )
    parser_calibrate.add_argument(
        "--n", type=int, default=20, help="需要采集的图像数量"
    )
    parser_calibrate.add_argument(
        "--use_local", action="store_true", help="使用本地图片进行标定"
    )

    # --stereo_calibrate 子命令
    parser_stereo_calibrate = subparsers.add_parser("stcal", help="校准双目相机")
    parser_stereo_calibrate.add_argument(
        "--cols", type=int, required=True, help="棋盘格的列数"
    )
    parser_stereo_calibrate.add_argument(
        "--rows", type=int, required=True, help="棋盘格的行数"
    )
    parser_stereo_calibrate.add_argument(
        "--n", type=int, default=20, help="需要采集的图像数量"
    )
    parser_stereo_calibrate.add_argument(
        "--use_local", action="store_true", help="使用本地图片进行标定"
    )

    # --show_undist 子命令
    subparsers.add_parser("show_undist", help="显示校正后的相机图像")

    # --show_disparity 子命令
    subparsers.add_parser("show_disparity", help="显示视差图")

    # --help 子命令
    subparsers.add_parser("help", help="帮助信息")

    return parser


def main():
    """主函数，初始化命令调用者并启动 REPL"""

    # 共享状态
    shared_state = {
        "left_camera_driver": None,  # 相机对象
        "right_camera_driver": None,
        "left_camera_queue": Queue(maxsize=10),  # 图像队列
        "right_camera_queue": Queue(maxsize=10),
        "cameras_started": False,  # 启动相机标志
    }

    # 命令映射
    command_map = {
        "start": StartCommand(shared_state),
        "stop": StopCommand(shared_state),
        "show": ShowImagesCommand(shared_state),
        "cal": CalibrateCommand(shared_state),
        "stcal": StereoCalibrateCommand(shared_state),
        "show_undist": ShowUndistImagesCommand(shared_state),
        "show_disparity": ShowDisparityCommand(shared_state),
    }

    invoker = CommandInvoker()
    for command_name, command_instance in command_map.items():
        invoker.register(command_name, command_instance)

    parser = create_parser()

    while True:
        try:
            # 读取用户输入
            user_input = input(">>>").strip().split()
            if not user_input:
                continue

            if user_input[0] == "exit":
                print("退出程序")
                break

            # 解析用户输入的命令
            args = parser.parse_args(user_input)

            if args.command in command_map:
                invoker.execute(args.command, args)
            else:
                print(f"无法识别的命令: {args.command}")
                parser.print_help()

        except SystemExit as e:
            # 捕获 SystemExit 异常并打印调试信息，但不退出程序
            print(f"解析错误: {e}")
            parser.print_help()
        except KeyboardInterrupt:
            print("\n程序中断")
            break
        finally:
            # 添加一个小的延迟，减少CPU占用
            time.sleep(0.1)


if __name__ == "__main__":
    main()
